#include <boost/thread.hpp>
#include <stdlib.h>
#include <iostream>
#include <string>
#include <cv.h>
#include <highgui.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <math.h>


#define Kp 0.15

using namespace cv;
using namespace std;

int redN = 0;
int greenN = 0;
int blueN = 0;
int yellowN = 0;
int burger=3000;
float fGamma = 1 / 1.6;
double vec = 0;
double angle_vec=0;
const size_t inWidth = 300;
const size_t inHeight = 300;
const float WHRATIO = inWidth / (float)inHeight;
float GRAD = 0.1;


void GammaCorrection(Mat src, Mat &dst, float fGamma);
void Separate(Mat src,Mat &dst);
void Command(Mat src,geometry_msgs::Twist &cmd_red); 
double SideCalc(Point a,Point b);
double Grad(Point a,Point b);
double Damp(double vec,double last_vec);

int main(int argc, char **argv) 
{
	ros::init(argc,argv,"ColorMove");                                     //初始化 ROS 节点
	ros::NodeHandle n;

	ros::Publisher cmd_pub;
	cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 5);            //定义速度发布器

	VideoCapture capture;
	geometry_msgs::Twist twist;
	capture.open(1);                                                      //打开 zed 相机
	waitKey(100);

	if (!capture.isOpened()) 
	{
		printf("摄像头没有正常打开，重新插拔工控机上当摄像头\n");
	}

	Mat src_frame;
	ros::Rate loop_rate(10);

	while (ros::ok()) 
	{
		capture.read(src_frame); 
		if (src_frame.empty()) 
		{
			break;
        	}
		Mat Gamma_Image,Gaussian_Image,Separate_Image;		
		GammaCorrection(src_frame, Gamma_Image, fGamma);              //伽玛矫正
		GaussianBlur(Gamma_Image,Gaussian_Image,Size(3,3),3,3);         //高斯滤波
		Separate(Gaussian_Image, Separate_Image);                       //颜色分割
		imshow("Separate_Image",Separate_Image);
		
		waitKey(10);

		twist.linear.x = 0;//线速度 
            	twist.linear.y = 0;
            	twist.linear.z = 0;
            	twist.angular.x = 0;
            	twist.angular.y = 0;
            	twist.angular.z = 0;//角速度

		Command(Separate_Image, twist);
            	
            	cmd_pub.publish(twist); //发布消息
		loop_rate.sleep();
    	}
	return 0;
}

//伽玛校正(fGamma = 1 / 1.6)
void GammaCorrection(Mat src, Mat &dst, float fGamma)
{
	unsigned char lut[256];
	for( int i = 0; i < 256; i++ )
	{
		lut[i] = saturate_cast<uchar>(pow((float)(i/255.0), fGamma) * 255.0f);//防止颜色溢出操作
	}	

	dst = src.clone();
	const int channels = dst.channels();

	switch(channels)
	{
		//单通道
		case 1:
			{
				MatIterator_<uchar> it, end;
				for( it = dst.begin<uchar>(), end = dst.end<uchar>(); it != end; it++ )
					*it = lut[(*it)];
				break;
			}
		//三通道
		case 3: 
			{
 
				MatIterator_<Vec3b> it, end;
				for( it = dst.begin<Vec3b>(), end = dst.end<Vec3b>(); it != end; it++ )
				{
					(*it)[0] = lut[((*it)[0])];
					(*it)[1] = lut[((*it)[1])];
					(*it)[2] = lut[((*it)[2])];
				}
				break;
			}
	}
}

//颜色分割
void Separate(Mat src,Mat &dst)
{		
	//转换到HSV色度空间
	Mat hsv;
	cvtColor(src,hsv,COLOR_BGR2HSV);

	//提取颜色，随机调
	//inRange(hsv, Scalar(0,0, 0), Scalar(180,255, 46), dst);                        //颜色分割
	inRange(hsv, Scalar(100,43, 46), Scalar(124,255, 255), dst);   
	//开运算+闭操作
	Mat element=getStructuringElement(MORPH_RECT, Size(15,15));                    //先开操作再闭操作
	morphologyEx(dst, dst, MORPH_OPEN, element);
	morphologyEx(dst, dst, MORPH_CLOSE, element);
}

void Command(Mat src,geometry_msgs::Twist &cmd_red)                                    //根据分离出来的图像决定dashgo的行动
{
	//寻找轮廓
	vector<vector<Point>> contours;
	vector<Vec4i> hierarchy;

	findContours(src, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));        //找轮廓

	//对每个找到的轮廓创建可倾斜的边界框
	vector<RotatedRect> minRect(contours.size());
	for (int i = 0; i < contours.size(); i++)
	{
	    	minRect[i] = minAreaRect(Mat(contours[i]));
	}

	bool flag=false;                                                               //是否有且仅有一个符合条件的边框
	Point center(0,0);                                                             //轮廓中心点
	double Area=-1;
	//筛选边框并且根据有效轮廓决定运动参数（如果有多个，则参数无意义）
	//绘出有效轮廓及其可倾斜的边界框     
	Mat drawing = Mat::zeros(src.size(), CV_8UC3);
	for (int i = 0; i< contours.size(); i++)
	{
	    	Point2f rect_points[4];                                                 //轮廓四个拐点
		minRect[i].points(rect_points);
		//计算方框相邻边长和面积
		double side1=SideCalc(rect_points[0],rect_points[1]);
		double side2=SideCalc(rect_points[1],rect_points[2]);
		double area=side1*side2;
		
		//判断轮廓形状是否是要求长方形
		if(side1/side2>WHRATIO && side2/side1>WHRATIO)
		{
			continue;
		}

		//判断长方形长边的斜率
		double longGrad;
		if(side1 > side2)
		{
			longGrad=Grad(rect_points[0],rect_points[1]);
		}
		else 
		{
			longGrad=Grad(rect_points[1],rect_points[2]);
		}
		
		if(longGrad < GRAD)
		{
			continue;                                                     //如果过于平缓，则淘汰该矩形
		}
		
		//判断轮廓的大小，并且根据可能轮廓判断轮廓与机器人距离
		if(area >= 130000 || area < 3000)
		{
			continue;                                                      //如果距离过远或者过近则不运动，同时排除掉过小的边框
		}
		
		//寻找轮廓中心点，并绘制边框
		for (int j = 0; j < 4; j++)
	    	{            
	    		line(drawing, rect_points[j], rect_points[(j + 1) % 4], Scalar(0, 0, 255), 1, 50);
	    		center.x+=rect_points[j].x;
	    		center.y+=rect_points[j].y;
	 	}
		center.x/=4;
		center.y/=4;
		circle(drawing,center,2,Scalar(0,0,255));
                if(Area > -1)//已有有效边框
		{
			Area = -1;
			break;
		}
		else Area=area;
	}
        imshow("Contours",drawing);
    	//制定机器人指令
    	if(Area == -1)                                                                  //视野没有有效轮廓
    	{
    		return ;
    	}
    	//判断机器人速度
	double ref_vec = 1.0f * (130000 - Area) / 260000.0f;                             //目标速度
	vec = Damp(ref_vec , vec);                                                       //算出阻尼后的速度
	cmd_red.linear.x = vec;
    	//判断机器人前进的方向
    	double centerL = drawing.cols*5/11;
    	double centerR = drawing.rows*6/11;
	double ref_angle_vec = 0;
	if(center.x <= centerL)
	{
		ref_angle_vec=(centerL - center.x) / centerL;                                 //左边
	}
	else if(center.x > centerR)
	{
		ref_angle_vec = (centerR - center.x) / centerL;                           //右边
	}
	angle_vec=Damp(ref_angle_vec , angle_vec);                                          //算出阻尼后的速度
	cmd_red.angular.z = angle_vec;
}

//两点间距离
double SideCalc(Point a, Point b)
{
	return sqrtf(powf(a.x - b.x, 2)+powf(a.y - b.y, 2));
}

//直线的斜率
double Grad(Point a,Point b)
{
	if(a.x==b.x)
        return 0;
	return fabs(a.y - b.y)/fabs(a.x - b.x);
}

//阻尼函数
double Damp(double vec,double last_vec)
{
	double error = vec-last_vec;
	double output = last_vec+error * Kp;
	return output;
}





